import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped  #消息接口
from tf2_ros import StaticTransformBroadcaster #静态坐标发布器
from tf_transformations import quaternion_from_euler #欧拉角转四元数函数 
import math #角度转弧度 函数
class StaticTFBroadcaster(Node):
    def __init__(self):
        super().__init__('static_tf_broadcaster')
        self.broadcaster_ = StaticTransformBroadcaster(self) #创建静态坐标发布器对象
        self.publish_static_tf() #发布静态坐标变换

    def publish_static_tf(self):
        #发布静态TF，从base_link到camera_link之间的左边变换
        transform = TransformStamped()
        transform.header.stamp = self.get_clock().now().to_msg() #获取当前时间
        transform.header.frame_id = 'base_link' #父坐标系
        transform.child_frame_id = 'camera_link' #子坐标系
        transform.transform.translation.x = 0.3 #x轴平移0.3米
        transform.transform.translation.y = 0.5 #y轴平移0.5米
        transform.transform.translation.z = 0.6 #z轴平移0.6米

        #欧拉角转换为四元数
        roll, pitch, yaw = math.radians(180.0), math.radians(0.0), math.radians(0.0)  #math.radians(180.0)将 180角度转换为弧度
        q = quaternion_from_euler(roll, pitch, yaw)   #将欧拉角转换为四元数 元组,这里的q 就是一个数组（q = x,y,z,w）
        transform.transform.rotation.x = q[0] #x轴旋转
        transform.transform.rotation.y = q[1] #y轴旋转
        transform.transform.rotation.z = q[2] #z轴旋转
        transform.transform.rotation.w = q[3] #w轴旋转
        self.broadcaster_.sendTransform(transform) #发布静态坐标变换
        self.get_logger().info(f'Published static TF from base_link to camera_link:{transform}')    

def main():
    rclpy.init()
    node = StaticTFBroadcaster()
    rclpy.spin(node)
    rclpy.shutdown()